#include "robotLac.h"
#include "Servo.h"

robotLac robot(28,1);
Infrared_Sensor infI(A1), infD(A0);
int sensorI, sensorD;

void setup(){
  robot.begin();
  robot.detener();
}

void loop (){  
  sensorI=infI.read();
  sensorD=infD.read();
  
  if(sensorI >= 500 && sensorD >= 500)
    robot.avanzar();
  else if(sensorI >= 500 && sensorD < 500)
    robot.antihorario();
  else if(sensorI < 500 && sensorD >= 500)
    robot.horario();
  // llegué a una curva muy abrupta o al final del recorrido
  else{
    int timeI = 0;
    int timeD = 0;
    timeI = millis();
    while(infI.read() < 500)
      robot.antihorario();
    timeI = millis() - timeI;
    
    timeD = millis();
    while(infD.read() < 500)
      robot.horario();
    timeD = millis() - timeD - timeI;
    if(timeI <= timeD){
      while(infI.read() < 500)
        robot.antihorario();
    }
    else{
      while(infD.read() < 500)
        robot.horario();
    }
  }
}
